/*
 *  ump.c
 *  UMPTest
 *
 *  Created by Jonathan Diehl on 03.03.10.
 *  Copyright 2010 RWTH. All rights reserved.
 *
 */

#include "ump.h"
#include <stdlib.h>
#include <math.h>

// globals
ump_touch_list *prev_touch_list = NULL;


// read touch data
int ump_read_touches(ump_context *umpc, ump_touch_list **touch_list, int touchesLength)
{
	int x, y, dx, dy, nx, ny;
	int i, ni;
	int weight, isTouch;
	int numTouches = 0;
	ump_touch_list *touch_list_entry;
	float dd;
	
	// init touch_list
	*touch_list = NULL;
	
	// go through the grid
	// each local maximum is a touch
	// the actual touch position is moved towards all neighbors according to their pressure
	for(i = 0; i < umpc->length; i++) {
		
		// skip empty values
		if(umpc->data[i] == 0) continue;
		
		// get x/y
		x = i % umpc->width;
		y = (i - x) / umpc->width;
		
		// move towards each neighbor
		dx = 0.0;
		dy = 0.0;
		weight = umpc->data[i];
		
		isTouch = 1;
		for(ny = -1; ny < 2; ny++) {
			for(nx = -1; nx < 2; nx++) {
				
				// check for identity
				if(nx == 0 && ny == 0) continue;
				
				// check bounds
				if(x + nx < 0 || x + nx > umpc->width || y + ny < 0 || y + ny > umpc->height) continue;
				
				// get neighbor index
				ni = x+nx + (y+ny)*umpc->width;
				
				// not a maximum if neighbor is bigger (left same neighbor wins)
				if(umpc->data[ni] > umpc->data[i] ||(umpc->data[ni] == umpc->data[i] && ni < i)) {
					isTouch = 0;
					break;
				}
				
				// reject measurement below pressure threshold
				if(umpc->data[i] < umpc->touch_threshold) {
					isTouch = 0;
					break;
				}
				
				// compute weighted position adjustment
				dx += nx * umpc->data[ni];
				dy += ny * umpc->data[ni];
				weight += umpc->data[ni];
			}
		}
		
		// we found a touch
		if(isTouch) {
			
			if(*touch_list == NULL) {
				*touch_list = malloc(sizeof(ump_touch_list));
				touch_list_entry = *touch_list;
			} else {
				touch_list_entry->next = malloc(sizeof(ump_touch_list));
				touch_list_entry = touch_list_entry->next;
			}
			
			// set next item to NULL
			touch_list_entry->next = NULL;
			
			// adjust coordinates by the weighted sum of all neighbors
			touch_list_entry->touch.x = (x + (float)dx / weight) / umpc->width;
			touch_list_entry->touch.y = (y + (float)dy / weight) / umpc->height;
			
			// adjust pressure by distance from true touch center
			dd = sqrt((dx*dx+dy*dy) / (float)(weight*weight));
			touch_list_entry->touch.pressure = umpc->data[i] / 254.0 + dd / 2.0;
			
			// init prev_touch
			touch_list_entry->prev_touch.pressure = 0.0;
			
			numTouches++;
		}
		   
	}
	
	// link with previous touches
	ump_touch_list *prev_list_entry;
	ump_touch *best_match[numTouches];
	float distance, best_distance[numTouches];
	i = 0;

	// find the previous touch with the smallest distance for each touch
	for(touch_list_entry = *touch_list; touch_list_entry != NULL; touch_list_entry = touch_list_entry->next) {
		best_distance[i] = 1.0;
		best_match[i] = NULL;
		for(prev_list_entry = prev_touch_list; prev_list_entry != NULL; prev_list_entry = prev_list_entry->next) {
			distance = pow(touch_list_entry->touch.x - prev_list_entry->touch.x, 2.0) + pow(touch_list_entry->touch.y - prev_list_entry->touch.y, 2.0);
			if(best_distance[i] > distance) {
				best_distance[i] = distance;
				best_match[i] = &prev_list_entry->touch;
			}
		}
		i++;
	}
	
	// assign the previous touch
	int j;
	i = 0;
	for(touch_list_entry = *touch_list; touch_list_entry != NULL; touch_list_entry = touch_list_entry->next) {
		
		// there is no previous touch
		if(best_distance[i] == 1.0) continue;
		
		// make sure no other touch is closer to the identified closest touch
		for(j = 0; j < numTouches; j++) {
			if(best_distance[j] < best_distance[i] && best_match[j] == best_match[i]) {
				best_distance[i] = 1.0;
				break;
			}
		}
		
		// assign closest touch as previous touch
		if(best_distance[i] < 1.0) {
			touch_list_entry->prev_touch.x = best_match[i]->x;
			touch_list_entry->prev_touch.y = best_match[i]->y;
			touch_list_entry->prev_touch.pressure = best_match[i]->pressure;
		}
		
		i++;
	}
	
	// update prev_touches_list
	ump_free_touches(prev_touch_list);
	prev_touch_list = *touch_list;
	
	return numTouches;
}

// free touch data
void ump_free_touches(ump_touch_list *touch_list)
{
	ump_touch_list *this_entry, *next_entry;
	
	this_entry = touch_list;
	while(this_entry != NULL) {
		next_entry = this_entry->next;
		free(this_entry);
		this_entry = next_entry;
	}
}

// read device data
int ump_read(ump_context *umpc, unsigned char *buffer, int bufferLength)
{
	int i;
	int f = 0;
	
	// read all characters from the buffer
	for(i = 0; i < bufferLength; i++) {
		
		// stop bit
		if(buffer[i] == 255) {
			umpc->read_index = 0;
			
		// no stop bit yet
		} else if(umpc->read_index == -1) {
			
		// too much data
		} else if(umpc->read_index >= umpc->length) {
			f = -1;
			umpc->read_index = -1;
			
		// read data
		} else {
			umpc->data[umpc->read_index++] = buffer[i];
		}
	}
	
	return f;
}

// initialize the ump interface
int ump_init(ump_context *umpc, int width, int height)
{
	int i;
	
	// assign parameters
	umpc->width = width;
	umpc->height = height;
	umpc->length = width * height;
	
	// initialize internal variables
	umpc->read_index = -1;
	
	// initialize data array and fill with 0s
	umpc->data = malloc(umpc->length);
	for(i = 0; i < umpc->length; i++) {
		umpc->data[i] = 0;
	}
	
	// initialize touch threshold to 1/64
	umpc->touch_threshold = 4;
	
	return 0;
}


// clean up
int ump_deinit(ump_context *umpc)
{
	free(umpc->data);
	return 0;
}
